
#include "PID.h"


/**
    PID 三个参数从某种意义上分别代表了调控的快(p比例)、准(i积分)、稳(d微分)
    
    位置式PID
        缺点：
            1、积分饱和：积分项的累加求和如果在Ki处于0时，误差会被不断的累加，
                当某次调参时，设置Ki>0时，会有一个很明显的抖动
                解决：当Ki==0时，对积分球和不进行累加
    增量式PID
        优点：
            1、不存在位置式PID的积分饱和问题
                因为out项的《输出限幅》 兼具《积分限幅》的功能
            2、把最终pid函数的数据结果out直接简单粗暴的限幅到电机输出的范围，不用考虑其他
            3、方便自动控制和手动控制平滑切换
                把Kp、Ki、Kd全都设置为0，手动修改out值
        缺点:
            Kp一项，具体是Kp(error0 - error1)  会引入历史信息
            如果每次存在微小的error变化，那么Kp项很难自动调节，因为依赖了error1，甚至有积分项的特性
            加上Ki项才能缓解这个问题
            所以增量式PID调节，不建议Ki项为0
            
*/
float pid(PID_TypeDef * p){

    p->error = p->target - p->current;
    
        
        
    p->error_sum += p->error;
        
      /*
    二、积分分离（应用于位置式pid控制电机位置场景）
        PD 控制器的缺点：
            1、target 与 current 之间总是存在误差无法消除
                这是因为误差过小导致Kp项无法对其进行修正
            2、用手小幅拨动转盘，发现能轻易改变电机位置（电机输出太小）
        引入积分项（Ki）后能解决问题一，但是会额外引入下列问题：
            1、由于电机定位控制在out达到target时，
                不需要额外的 积分项（Ki）来抵抗摩檫力，
                所以会导致积分项（Ki）输出之前累积过大值，出现过调
        解决方案：当误差值大于阈值的时候关闭Ki，反之则开启Ki
    */
    
    /*
        使用微分先行，对输出（当前值）加阻尼
    */
    
    
    /*
    一、积分限幅
    这里的积分上限设置为：
    当pid输出最大 rps ，且只有 Ki项不为0 时
    积分的和
    */
    if(p->error_sum  > p->sum_upper_limit){
        // 积分限幅 
        /**
        为解决位置式pid 积分饱和问题
        这里采用积分限幅功能
        幅值 200 由观察上位机一般情况下最大的 error_sum 来确定
        */
        p->error_sum  = p->sum_upper_limit;
    }else if(p->error_sum < p->sum_lower_limit){
        p->error_sum = p->sum_lower_limit;
    }
       
    /*
    一般不建议乘以dt 或者 除以dt 
    这会使得Kp Ki Kd 不在一个量级(因为Kp项与dt无关)，
    调参的时候不方便
    所以后续的dt传入进来的都是 1
    */
    p->out = p->Kp * p->error
                        + p->Ki * p->error_sum
                        + p->Kd * (p->error - p->last_error);
    
    p->last_error = p->error;
        
    
    
    if(p->out  > p->out_upper_limit){
        // 输出限幅
        p->out  = p->out_upper_limit;
    }else if(p->out < p->out_lower_limit){
        p->out = p->out_lower_limit;
    }
    
    return p->out;
}

